#ifndef URCONTROLARC_HPP
#define URCONTROLARC_HPP
#include "urcontrol.hpp"
#include "opencv2/opencv.hpp"


class URControlArc : public URControl
{
public:
    static cv::Vec6d ComputeTransform(const cv::Vec3d ptFrom[4], const cv::Vec3d ptTo[4])
    {
        cv::Vec3d ctFrom = (ptFrom[0] + ptFrom[1] + ptFrom[2] + ptFrom[3]) / 4;
        cv::Vec3d ctTo = (ptTo[0] + ptTo[1] + ptTo[2] + ptTo[3]) / 4;
        cv::Matx33d H = (ptFrom[0] - ctFrom)*((ptTo[0] - ctTo).t());
        H += (ptFrom[1] - ctFrom)*((ptTo[1] - ctTo).t());
        H += (ptFrom[2] - ctFrom)*((ptTo[2] - ctTo).t());
        H += (ptFrom[3] - ctFrom)*((ptTo[3] - ctTo).t());
        cv::Mat U, S, Vt;
        cv::SVDecomp(H, S, U, Vt);
        cv::Mat R0 = (U*Vt).t();
        cv::Matx33d R((double*)R0.ptr());
        //        std::cout<<R<<"\n";
        cv::Vec3d rxyz;
        cv::Rodrigues(R, rxyz);
        cv::Vec3d xyz = ctTo - R*ctFrom;
        return cv::Vec6d(xyz[0], xyz[1], xyz[2], rxyz[0], rxyz[1], rxyz[2]);
    }

    static cv::Vec6d cp(double x, double y, double z, double r, double a)
    {
        cv::Vec3d ptFinger[4]={
            cv::Vec3d(0,0,0),
            cv::Vec3d(1,0,0),
            cv::Vec3d(0,1,0),
            cv::Vec3d(0,0,1)
        };

        cv::Vec3d ori;
        ori[0]=x+r*cos(a);
        ori[1]=y+r*sin(a);
        ori[2]=z;

        cv::Vec3d zu(0,0,-1);
        cv::Vec3d xu(-cos(a),-sin(a),0);
        cv::Vec3d yu=zu.cross(xu);

        cv::Vec3d ptBase[4]={
            ori,
            ori+xu,
            ori+yu,
            ori+zu
        };

        return ComputeTransform(ptFinger,ptBase);
    }

    static std::string RotateAroundZ(cv::Vec3d center, double radius, double startRad, double endRad,
                                     double a=0.02, double v=0.1, double r=0.025)
    {
        double step=0.2;
        double dRad=endRad-startRad;
        if(dRad<0)
            step=-step;
        int nseg=dRad/step;
        nseg++;
        double halfStep=dRad/nseg/2;

        cv::Vec6d urp0=cp(center[0],center[1],center[2],radius,startRad);
        std::string cmd0=URScript::SetTCPPoseScript(urp0[0],urp0[1],urp0[2],urp0[3],urp0[4],urp0[5],a,v,'l');
        std::string cmd=URScript::SetTCPPoseScript(urp0[0],urp0[1],urp0[2],urp0[3],urp0[4],urp0[5],a,v,'p',r);
        for(int i=1;i<=nseg;i++)
        {
            cv::Vec6d urp1=cp(center[0],center[1],center[2],radius,startRad+halfStep*(2*i-1));
            cv::Vec6d urp2=cp(center[0],center[1],center[2],radius,startRad+halfStep*(2*i));
            cmd+=URScript::SetTCPArcScript(urp1[0],urp1[1],urp1[2],urp1[3],urp1[4],urp1[5],
                    urp2[0],urp2[1],urp2[2],urp2[3],urp2[4],urp2[5],r,a,(i+3<nseg?v:0.1*v));
        }

        cv::Vec6d urp3=cp(center[0],center[1],center[2],radius,endRad);
        std::string cmd1=URScript::SetTCPPoseScript(urp3[0],urp3[1],urp3[2],urp3[3],urp3[4],urp3[5],a,v,'l');
        return cmd0+cmd+cmd1;
    }

    static void test()
    {
        char ipUR[]="192.168.1.13";
        char ipPC[]="192.168.1.14";

        cv::Vec3d center(-0.5,0,0.165);
        double r=0.245;
        std::string cmd=RotateAroundZ(center,r,0,CV_2PI);

        cmd+=RotateAroundZ(center,r,CV_2PI,0);
        //std::cout<<cmd<<"\n\n"<<std::flush;
        ScriptCommand(cmd,ipUR,ipPC);
    }

};

#endif // URCONTROLARC_HPP
